/**
 * 2024/03/15
 * ssyyw
 * mavproxy_ros1的飞行控制节点，根据当前飞行计划或指令，输出飞行控制命令到mavros
 * 
*/

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <string>
#include "mavrosproxy_ros1/FlightCmd.h"

using namespace std;

// offboard模式支持的mavlink消息就这几种，如果想要自定义消息，需要改PX4代码，再让PX4将其作为uORB主题发布。
const int DEFAULT_QUEUE_SIZE = 10;
const float ENABLE_MESSAGE_DURATION = 5.0;

int global_task_index = 0;

void CmdCallback(const mavproxy_ros1::FlightCmd::ConstPtr &msg){
    drone_current_cmd = *msg;
}

void HandleCmd(){
    if (drone_last_cmd == drone_current_cmd){
        return;
    }
    if (dorne_current_cmd == ){}
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "flight_planner");

    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}